#include "Robot.h"

void Robot::AutonomousInit() {
	shift.Set(true);
	shiftRetract.Set(false);



	autonMode = 0;

	if(autonMode1.Get()) {
		if(autonMode2.Get()) {
			autonMode = 3;
		}
		else {
			autonMode = 1;
		}
	}
	else if(autonMode2.Get()) {
		autonMode = 2;
	}
	else {
		autonMode = 0;
	}


	rightDrive.Reset();
	leftDrive.Reset();
	liftEncoder.Reset();

	rspeed = 0;
	lspeed = 0;

	autonDSDone = false;

	autonPPStage = 0;
	autonPPCount = 0;
	autonPPDone = false;

	autonPDStage = 0;
	autonPDDone = false;

	autonNSStage = 0;

	autonCount = 0;

	gyro.Reset();
	comp.Start();
}

void Robot::AutonomousPeriodic() {

	if(autonCount % 30 == 0)
	{
		SmartDashboard::PutString("DB/String 6", std::to_string(gyro.GetAngle()));
		SmartDashboard::PutString("DB/String 7", std::to_string(leftDrive.GetDistance()));
		SmartDashboard::PutString("DB/String 8", std::to_string(rightDrive.GetDistance()));
		SmartDashboard::PutString("DB/String 9", std::to_string(liftEncoder.GetDistance()));
	}

	switch (autonMode) {

	case 1:
		if(!autonDSDone) {
			autonDriveStraight();
		}
		break;

	case 2:
		if(!autonPDDone){
			autonPickUpAndDrive();
		}
		break;
	case 3:
		if (!autonPPDone) {
			autonPickUpAndPush();
		}
		break;
	}

	autonCount++;

}
void Robot::autonPickUpAndDrive(){
	switch(autonPDStage) {
	case 0:
		rightArm.Set(false);
		rightArmRetract.Set(true);
		leftArm.Set(false);
		leftArmRetract.Set(true);
		autonPDStage += 1;
		break;
	case 1:
		liftMotor.Set(1.0);
		if(liftEncoder.GetDistance() > 10) {
			liftMotor.Set(0.0);
			autonPDStage += 1;
		}
		break;

	case 2:
		if(gyro.GetAngle() < 87) {
			setLeftSide(-.4);
			setRightSide(.4);
		}
		else if(gyro.GetAngle() > 92){
			setLeftSide(.2);
			setRightSide(-.2);
		}
		else{
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPDStage +=1;
			leftDrive.Reset();
			rightDrive.Reset();

		}
		break;
	case 3:
		if((leftDrive.GetDistance() + rightDrive.GetDistance())/2 < 80	) {
			rspeed = .75;
			lspeed = .75;
			if(gyro.GetAngle() > 90) {
				rspeed -= fabs(gyro.GetAngle()-90)/30;
			}
			else if(gyro.GetAngle() < 80) {
				lspeed -= fabs(gyro.GetAngle()-90)/30;
			}

			setRightSide(rspeed);
			setLeftSide(lspeed);

		}
		else /*if((leftDrive.Get() + rightDrive.Get())/2 <= 36050 && (leftDrive.Get() + rightDrive.Get())/2 >= 36060)*/ {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPDStage += 1;
		}
		break;
	case 4:
		if(gyro.GetAngle() > 2) {
			setLeftSide(.4);
			setRightSide(-.4);
		}
		else if(gyro.GetAngle() < -2){
			setLeftSide(-.2);
			setRightSide(.2);
		}
		else{
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPDStage +=1;
			leftDrive.Reset();
			rightDrive.Reset();

		}
		break;

	case 5:
		if((leftDrive.GetDistance() + rightDrive.GetDistance())/2 < 10	) {
			rspeed = .2;
			lspeed = .2;
			if(gyro.GetAngle() > 0) {
				rspeed -= fabs(gyro.GetAngle())/30;
			}
			else if(gyro.GetAngle() < 0) {
				lspeed -= fabs(gyro.GetAngle())/30;
			}

			setRightSide(rspeed);
			setLeftSide(lspeed);

			autonPPCount = 0;

		}
		else /*if((leftDrive.Get() + rightDrive.Get())/2 <= 36050 && (leftDrive.Get() + rightDrive.Get())/2 >= 36060)*/ {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPDStage += 1;
			autonPDDone = true;
		}
		break;

	}

}
void Robot::autonDriveStraight() {
	double rspeed = 0.75;
	double lspeed = 0.75;

	if(gyro.GetAngle() > 0) {
		rspeed -= gyro.GetAngle()/30;
	}
	else if(gyro.GetAngle() < 0) {
		lspeed -= gyro.GetAngle()/30;
	}


	setRightSide(rspeed);
	setLeftSide(lspeed);
	if( (rightDrive.GetDistance() + leftDrive.GetDistance())/2 > 85) {
		setRightSide(0.0);
		setLeftSide(0.0);
		autonDSDone = true;
	}


}
void Robot::autonPickUpAndPush() {
	switch(autonPPStage) {
	case 0:

		rightArm.Set(false);
		rightArmRetract.Set(true);
		leftArm.Set(false);
		leftArmRetract.Set(true);
		autonPPCount += 1;

		if(autonPPCount > 5) {
			autonPPStage += 1;
		}
		break;

	case 1:
		liftMotor.Set(1.0);
		if(liftEncoder.GetDistance() > 21) {
			liftMotor.Set(0.0);
			autonPPStage += 1;

			rightDrive.Reset();
			leftDrive.Reset();
		}
		break;
	case 2:


		rspeed = -0.5;
		lspeed = -0.5;

		if(gyro.GetAngle() < 0) {
			rspeed += gyro.GetAngle()/30;
		}
		else if(gyro.GetAngle() > 0) {
			lspeed += gyro.GetAngle()/30;
		}


		setRightSide(rspeed);
		setLeftSide(lspeed);

		if( (rightDrive.GetDistance() + leftDrive.GetDistance())/2 < 2) {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPPStage += 1;
		}

		break;

	case 3:
		if(gyro.GetAngle() > -83) {
			setLeftSide(.6);
			setRightSide(-.6);
			autonPPCount = 0;
		}
		else if(gyro.GetAngle() < -92){
			setLeftSide(.2);
			setRightSide(-.2);
			autonPPCount = 0;
		}
		else {
			setLeftSide(0.0);
			setRightSide(0.0);
			autonPPCount += 1;
			if(autonPPCount > 2) {
				autonPPStage += 1;
			}
			leftDrive.Reset();
			rightDrive.Reset();

		}
		break;
	case 4:
		if((leftDrive.GetDistance() + rightDrive.GetDistance())/2 < 2) {
			rspeed = .5;
			lspeed = .5;

			if(gyro.GetAngle() > -90) {
				rspeed -= fabs(gyro.GetAngle()+90)/30;
			}
			else if(gyro.GetAngle() < -90) {
				lspeed -= fabs(gyro.GetAngle()+90)/30;
			}

			setRightSide(rspeed);
			setLeftSide(lspeed);
			autonPPCount = 0;

		}

		else {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPPCount += 1;
			//if(autonPPCount > 100) {
				autonPPStage += 1;
			//}
		}
		break;
	case 5:
		if(gyro.GetAngle() < 5) {
			setLeftSide(.10);
			setRightSide(.70);
			autonPPCount = 0;
		}
		else {
			setLeftSide(0.0);
			setRightSide(0.0);
			autonPPCount += 1;
			if(autonPPCount > 2) {
				autonPPStage += 1;
				rightDrive.Reset();
				leftDrive.Reset();
			}
		}
		break;
	case 6:
		if((leftDrive.GetDistance() + rightDrive.GetDistance())/2 < 80	) {
			rspeed = .8;
			lspeed = .8;
			if(gyro.GetAngle() > 0) {
				rspeed -= fabs(gyro.GetAngle())/30;
			}
			else if(gyro.GetAngle() < 0) {
				lspeed -= fabs(gyro.GetAngle())/30;
			}

			setRightSide(rspeed);
			setLeftSide(lspeed);

			autonPPCount = 0;

		}
		else {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPPCount += 1;
			if(autonPPCount > 2) {
				autonPPStage += 1;
				liftMotor.Set(0.0);

			}
		}
		break;
	case 7:
		setRightSide(.25);
		setLeftSide(-.25);
		if(gyro.GetAngle() > 90) {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonPPStage += 1;
			autonPPDone = true;

		}
		break;
//	case 8:
//		liftMotor.Set(-1.0);
//		if(liftEncoder.Get() < 0) {
//			liftMotor.Set(0.0);
//			rightArm.Set(true);
//			rightArmRetract.Set(false);
//			leftArm.Set(true);
//			leftArmRetract.Set(false);
//			autonPPDone = true;
//		}


	}

}

void Robot::autonNemesisStrat() {
	switch(autonNSStage) {
	case 0:

		rightArm.Set(false);
		rightArmRetract.Set(true);
		leftArm.Set(false);
		leftArmRetract.Set(true);
		autonNSStage += 1;
		break;

	case 1:
		liftMotor.Set(1.0);
		if(liftEncoder.GetDistance() > 5) {
			liftMotor.Set(0.0);
			autonNSStage += 1;

			rightDrive.Reset();
			leftDrive.Reset();
		}
		break;

	case 2:


		if((rightDrive.GetDistance() + leftDrive.GetDistance())/2 > -60) {
			lspeed = -.75;
			rspeed = -.75;

			if(gyro.GetAngle() < 0) {
				rspeed += fabs(gyro.GetAngle())/30;
			}
			else if(gyro.GetAngle() > 0) {
				lspeed += fabs(gyro.GetAngle())/30;
			}
			setRightSide(rspeed);
			setLeftSide(lspeed);

		}
		else {
			setRightSide(0.0);
			setLeftSide(0.0);
			autonNSDone = true;
			autonNSStage += 1;
		}
		break;
	}
}
